#include <ros/ros.h>
#include <ros/console.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <cstdlib> // Needed for rand()
#include <ctime> // Needed to seed random number generator with a time value


class RandomWalk {

public:
  // Construst a new RandomWalk object and hook up this ROS node
  // to the simulated robot's velocity control and laser topics
  RandomWalk(ros::NodeHandle& nh) : fsm(FSM_MOVE_FORWARD),rotateStartTime(ros::Time::now()),rotateDuration(0.f) {
    // Initialize random time generator
    srand(time(NULL));
    // Advertise a new publisher for the simulated robot's velocity command topic
    // (the second argument indicates that if multiple command messages are in
    //  the queue to be sent, only the last command will be sent)
    commandPub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    // Subscribe to the simulated robot's laser scan topic and tell ROS to call
    // this->commandCallback() whenever a new message is published on that topic
    laserSub = nh.subscribe("base_scan", 1, &RandomWalk::commandCallback, this);
  }

  // Send a velocity command
	void move(double linearVelMPS, double angularVelRadPS)
	{
		geometry_msgs::Twist msg; // The default constructor will set all commands to 0
		msg.linear.x = linearVelMPS;
		msg.angular.z = angularVelRadPS;
		commandPub.publish(msg);
	}

	float findClosestRange(const sensor_msgs::LaserScan::ConstPtr& msg)
	{
		const float minAcceptableRange = msg->range_min;
		const float maxAcceptableRange = msg->range_max;

		float result = maxAcceptableRange;

		unsigned int minIndex = ceil((-SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		unsigned int maxIndex = ceil((SCAN_ANGLE_RAD - msg->angle_min) / msg->angle_increment);
		//ROS_INFO_STREAM("#ranges: " << (msg->angle_max - msg->angle_min) / msg->angle_increment);

		for (unsigned int currIndex = minIndex; currIndex < maxIndex; currIndex++)
		{
			if (msg->ranges[currIndex] < result
			 && msg->ranges[currIndex] >= minAcceptableRange)
			{
				result = msg->ranges[currIndex];
			}
		}
		return result;
	}

	// Process the incoming laser scan message
	void commandCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
	{
		if (fsm == FSM_MOVE_FORWARD)
		{
			float closestRange = findClosestRange(msg);

			//ROS_INFO_STREAM("Range: " << closestRange);

			float timeToRotate = (((rand()%360)*M_PI)/180.f)/ROTATE_SPEED_RADPS;

			/////////////////////// ANSWER CODE BEGIN ///////////////////
			if (closestRange < PROXIMITY_RANGE_M)
			{
				fsm = FSM_ROTATE;
				rotateStartTime = ros::Time::now();
				rotateDuration = ros::Duration(timeToRotate);
			}
			/////////////////////// ANSWER CODE END ///////////////////
		}
	}

		// Main FSM loop for ensuring that ROS messages are
		// processed in a timely manner, and also for sending
		// velocity controls to the simulated robot based on the FSM state
		void spin()
		{
			ros::Rate rate(10); // Specify the FSM loop rate in Hz
			while (ros::ok())
			{ // Keep spinning loop until user presses Ctrl+C
			// TODO: Either call:
			//
			//       - move(0, ROTATE_SPEED_RADPS); // Rotate right
			//
			//       or
			//
			//       - move(FORWARD_SPEED_MPS, 0); // Move foward
			//
			//       depending on the FSM state; also change the FSM state when appropriate
			/////////////////////// ANSWER CODE BEGIN ///////////////////

			if (fsm == FSM_MOVE_FORWARD)
			{
				move(FORWARD_SPEED_MPS, 0); // Move foward
			}
			else if (fsm == FSM_ROTATE)
			{
				move(0, ROTATE_SPEED_RADPS); // Rotate right

				ros::Time now = ros::Time::now();
				if(now > rotateStartTime + rotateDuration)
					fsm = FSM_MOVE_FORWARD;
			}
			/////////////////////// ANSWER CODE END ///////////////////

			ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
			rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
		}
	}

  enum FSM {FSM_MOVE_FORWARD, FSM_ROTATE};
  // Tunable parameters

  // TODO: tune parameters as you see fit
  const static float SCAN_ANGLE_RAD = 30.f/180.f*M_PI;
  const static float PROXIMITY_RANGE_M = 0.4f; // Should be smaller than sensor_msgs::LaserScan::range_max
  const static float FORWARD_SPEED_MPS = 0.4f;
  const static float ROTATE_SPEED_RADPS = M_PI/2.f;


protected:
  ros::Publisher commandPub; // Publisher to the simulated robot's velocity command topic
  ros::Subscriber laserSub; // Subscriber to the simulated robot's laser scan topic
  FSM fsm; // Finite state machine for the random walk algorithm
  ros::Time rotateStartTime; // Start time of the rotation
  ros::Duration rotateDuration; // Duration of the rotation
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "random_walk"); // Initiate new ROS node named "random_walk"
  ros::NodeHandle n;
  RandomWalk walker(n); // Create new random walk object
  walker.spin(); // Execute FSM loop
  return 0;
}
